/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2024-03-20     huger       the first version
 */
#include "foc.h"
#include "Math/arm_math.h"

#include "Bsp/bsp_svpwm.h"
static uint8_t sector=0;

float ID_REF = 0.0F;
float IQ_REF = 0.0F;

CURRENT_ABC_DEF Current_Iabc;
CURRENT_ALPHA_BETA_DEF Current_Ialpha_beta;
VOLTAGE_ALPHA_BETA_DEF Voltage_Alpha_Beta;
TRANSF_COS_SIN_DEF Transf_Cos_Sin;
CURRENT_DQ_DEF Current_Idq;
VOLTAGE_DQ_DEF Voltage_DQ;

static uint8_t PWM4Direction = PWM2_MODE;

extern  TIM_HandleTypeDef htim1;

/**********************************************************************************************************
Clarke变换，输入Ia,Ib，得到Ialpha和Ibeta
**********************************************************************************************************/
/***************************************
功能：Clark变换
形参：三相电流以及alpha_beta电流
说明：由三相互差120度变换到两相互差90度
***************************************/
void Clarke_Transf(CURRENT_ABC_DEF Current_abc_temp,CURRENT_ALPHA_BETA_DEF* Current_alpha_beta_temp)
{
  Current_alpha_beta_temp->Ialpha = (Current_abc_temp.Ia - (Current_abc_temp.Ib + Current_abc_temp.Ic) * 0.5F) * 2.0F / 3.0F;
  Current_alpha_beta_temp->Ibeta = (Current_abc_temp.Ib - Current_abc_temp.Ic) * 0.866025388F * 2.0F / 3.0F;
}
/***************************************
功能：COS_SIN值计算
形参：角度以及COS_SIN结构体
说明：COS_SIN值计算
***************************************/
void Angle_To_Cos_Sin(float_t angle_temp,TRANSF_COS_SIN_DEF* cos_sin_temp)
{
  cos_sin_temp->Cos = arm_cos_f32(angle_temp);
  cos_sin_temp->Sin = arm_sin_f32(angle_temp);
}
/***************************************
功能：PARK变换
形参：alpha_beta电流、COS_SIN值、DQ轴电流
说明：交流变直流
***************************************/
void Park_Transf(CURRENT_ALPHA_BETA_DEF current_alpha_beta_temp,TRANSF_COS_SIN_DEF cos_sin_temp,CURRENT_DQ_DEF* current_dq_temp)
{
  current_dq_temp->Id = current_alpha_beta_temp.Ialpha * cos_sin_temp.Cos + current_alpha_beta_temp.Ibeta * cos_sin_temp.Sin;
  current_dq_temp->Iq = -current_alpha_beta_temp.Ialpha * cos_sin_temp.Sin + current_alpha_beta_temp.Ibeta * cos_sin_temp.Cos;
}
/***************************************
功能：反PARK变换
形参：DQ轴电压、COS_SIN值、alpha_beta电压
说明：直流变交流
***************************************/
void Rev_Park_Transf(VOLTAGE_DQ_DEF v_dq_temp,TRANSF_COS_SIN_DEF cos_sin_temp,VOLTAGE_ALPHA_BETA_DEF* v_alpha_beta_temp)
{
  v_alpha_beta_temp->Valpha = cos_sin_temp.Cos * v_dq_temp.Vd - cos_sin_temp.Sin * v_dq_temp.Vq;
  v_alpha_beta_temp->Vbeta  = cos_sin_temp.Sin * v_dq_temp.Vd + cos_sin_temp.Cos * v_dq_temp.Vq;
}

/***************************************
功能：SVPWM计算
形参：alpha_beta电压以及母线电压、定时器周期
说明：根据alpha_beta电压计算三相占空比
***************************************/
void SVPWM_Calc(VOLTAGE_ALPHA_BETA_DEF v_alpha_beta_temp,float_t Udc_temp,float_t Tpwm_temp)
{

  float_t Tcmp1,Tcmp2,Tcmp3,Tx,Ty,f_temp,Ta,Tb,Tc;
  uint16_t Tcmp4 ;
  uint32_t  hDeltaDuty;
  sector = 0;
  Tcmp1 = 0.0F;
  Tcmp2 = 0.0F;
  Tcmp3 = 0.0F;
  Tcmp4 = 0;//计算采样时机
  if (v_alpha_beta_temp.Vbeta > 0.0F)
  {
    sector = 1;
  }

  if ((1.73205078F * v_alpha_beta_temp.Valpha - v_alpha_beta_temp.Vbeta) / 2.0F > 0.0F) {
    sector += 2;
  }

  if ((-1.73205078F * v_alpha_beta_temp.Valpha - v_alpha_beta_temp.Vbeta) / 2.0F > 0.0F) {
    sector += 4;
  }

  switch (sector) {
  case 1:
    Tx = (-1.5F * v_alpha_beta_temp.Valpha + 0.866025388F * v_alpha_beta_temp.Vbeta) * (Tpwm_temp / Udc_temp);
    Ty = (1.5F * v_alpha_beta_temp.Valpha + 0.866025388F * v_alpha_beta_temp.Vbeta) * (Tpwm_temp / Udc_temp);
    break;

  case 2:
    Tx = (1.5F * v_alpha_beta_temp.Valpha + 0.866025388F * v_alpha_beta_temp.Vbeta) * (Tpwm_temp / Udc_temp);
    Ty = -(1.73205078F * v_alpha_beta_temp.Vbeta * Tpwm_temp / Udc_temp);
    break;

  case 3:
    Tx = -((-1.5F * v_alpha_beta_temp.Valpha + 0.866025388F * v_alpha_beta_temp.Vbeta) * (Tpwm_temp / Udc_temp));
    Ty = 1.73205078F * v_alpha_beta_temp.Vbeta * Tpwm_temp / Udc_temp;
    break;

  case 4:
    Tx = -(1.73205078F * v_alpha_beta_temp.Vbeta * Tpwm_temp / Udc_temp);
    Ty = (-1.5F * v_alpha_beta_temp.Valpha + 0.866025388F * v_alpha_beta_temp.Vbeta) * (Tpwm_temp / Udc_temp);
    break;

  case 5:
    Tx = 1.73205078F * v_alpha_beta_temp.Vbeta * Tpwm_temp / Udc_temp;
    Ty = -((1.5F * v_alpha_beta_temp.Valpha + 0.866025388F * v_alpha_beta_temp.Vbeta) * (Tpwm_temp / Udc_temp));
    break;

  default:
    Tx = -((1.5F * v_alpha_beta_temp.Valpha + 0.866025388F * v_alpha_beta_temp.Vbeta) * (Tpwm_temp / Udc_temp));
    Ty = -((-1.5F * v_alpha_beta_temp.Valpha + 0.866025388F * v_alpha_beta_temp.Vbeta) * (Tpwm_temp / Udc_temp));
    break;
  }

  f_temp = Tx + Ty;
  if (f_temp > Tpwm_temp) {
    Tx /= f_temp;
    Ty /= (Tx + Ty);
  }

  Ta = (Tpwm_temp - (Tx + Ty)) / 4.0F;
  Tb = Tx / 2.0F + Ta;
  Tc = Ty / 2.0F + Tb;

  PWM4Direction = PWM2_MODE;

  switch (sector) {
      case 1:
        Tcmp1 = Tb;
        Tcmp2 = Ta;
        Tcmp3 = Tc;

        if((uint16_t)(PWM_PERIOD-(uint16_t)Tcmp1)>TW_AFTER)   // tcmp1  占空比< PWM_PERIOD - TW_AFTER ,  adc采样占空比设置最大  pwm2_mode2
        {
            Tcmp4=PWM_PERIOD-10;
        }
        else     //  tcmp1  占空比 > PWM_PERIOD - TW_AFTER
        {
            hDeltaDuty = (uint16_t)(Tcmp1 - Tcmp2);
            if (hDeltaDuty > (uint16_t)(PWM_PERIOD-(uint16_t)Tcmp1)*2)    //a 、b占空比相差较大
            {
                    Tcmp4 = (uint16_t)Tcmp1 - TW_BEFORE; // Ts before Phase A         //提前tmcp1  TW_BEFORE ad 采样时间
            }
            else
            {
                Tcmp4 = (uint16_t)Tcmp1 + TW_BEFORE;
                if (Tcmp4 >= PWM_PERIOD)
                {
                    PWM4Direction=PWM1_MODE;
                    Tcmp4 = (2 * PWM_PERIOD) - Tcmp4-1;
                }
            }
        }

        break;
      case 2:
        Tcmp1 = Ta;
        Tcmp2 = Tc;
        Tcmp3 = Tb;

        if((uint16_t)(PWM_PERIOD-(uint16_t)Tcmp2)>TW_AFTER)
        {
            Tcmp4=PWM_PERIOD-10;
        }
        else
        {
            hDeltaDuty = (uint16_t)(Tcmp2 - Tcmp1);
            if (hDeltaDuty > (uint16_t)(PWM_PERIOD-(uint16_t)Tcmp2)*2)
            {
                    Tcmp4 = (uint16_t)Tcmp2 - TW_BEFORE; // Ts before Phase A
            }
            else
            {
                Tcmp4 = (uint16_t)Tcmp2 + TW_BEFORE;
                if (Tcmp4 >= PWM_PERIOD)
                {
                    PWM4Direction=PWM1_MODE;
                    Tcmp4 = (2 * PWM_PERIOD) - Tcmp4-1;
                }
            }
        }

        break;
      case 3:
        Tcmp1 = Ta;
        Tcmp2 = Tb;
        Tcmp3 = Tc;

        if((uint16_t)(PWM_PERIOD-(uint16_t)Tcmp2)>TW_AFTER)
        {
            Tcmp4=PWM_PERIOD-10;
        }
        else
        {
            hDeltaDuty = (uint16_t)(Tcmp2 - Tcmp3);
            if (hDeltaDuty > (uint16_t)(PWM_PERIOD-(uint16_t)Tcmp2)*2)
            {
                    Tcmp4 = (uint16_t)Tcmp2 - TW_BEFORE; // Ts before Phase A
            }
            else
            {
                Tcmp4 = (uint16_t)Tcmp2 + TW_BEFORE;
                if (Tcmp4 >= PWM_PERIOD)
                {
                    PWM4Direction=PWM1_MODE;
                    Tcmp4 = (2 * PWM_PERIOD) - Tcmp4-1;
                }
            }
        }

        break;
      case 4:
        Tcmp1 = Tc;
        Tcmp2 = Tb;
        Tcmp3 = Ta;

        if((uint16_t)(PWM_PERIOD-(uint16_t)Tcmp3)>TW_AFTER)
        {
            Tcmp4=PWM_PERIOD-10;
        }
        else
        {
            hDeltaDuty = (uint16_t)(Tcmp3 - Tcmp2);
            if (hDeltaDuty > (uint16_t)(PWM_PERIOD-(uint16_t)Tcmp3)*2)
            {
                Tcmp4 = (uint16_t)Tcmp3 - TW_BEFORE; // Ts before Phase A
            }
            else
            {
                Tcmp4 = (uint16_t)Tcmp3 + TW_BEFORE;
                if (Tcmp4 >= PWM_PERIOD)
                {
                    PWM4Direction=PWM1_MODE;
                    Tcmp4 = (2 * PWM_PERIOD) - Tcmp4-1;
                }
            }
        }

        break;
      case 5:
        Tcmp1 = Tc;
        Tcmp2 = Ta;
        Tcmp3 = Tb;

        if((uint16_t)(PWM_PERIOD-(uint16_t)Tcmp3)>TW_AFTER)
        {
            Tcmp4=PWM_PERIOD-10;
        }
        else
        {
            hDeltaDuty = (uint16_t)(Tcmp3 - Tcmp1);
            if (hDeltaDuty > (uint16_t)(PWM_PERIOD-(uint16_t)Tcmp3)*2)
            {
                    Tcmp4 = (uint16_t)Tcmp3 - TW_BEFORE; // Ts before Phase A
            }
            else
            {
                Tcmp4 = (uint16_t)Tcmp3 + TW_BEFORE;
                if (Tcmp4 >= PWM_PERIOD)
                {
                    PWM4Direction=PWM1_MODE;
                    Tcmp4 = (2 * PWM_PERIOD) - Tcmp4-1;
                }
            }
        }

        break;
      case 6:
        Tcmp1 = Tb;
        Tcmp2 = Tc;
        Tcmp3 = Ta;

        if((uint16_t)(PWM_PERIOD-(uint16_t)Tcmp1)>TW_AFTER)
        {
            Tcmp4=PWM_PERIOD-10;
        }
        else
        {
            hDeltaDuty = (uint16_t)(Tcmp1 - Tcmp3);
            if (hDeltaDuty > (uint16_t)(PWM_PERIOD-(uint16_t)Tcmp1)*2)
            {
                    Tcmp4 = (uint16_t)Tcmp1 - TW_BEFORE; // Ts before Phase A
            }
            else
            {
                Tcmp4 = (uint16_t)Tcmp1 + TW_BEFORE;
                if (Tcmp4 >= PWM_PERIOD)
                {
                    PWM4Direction=PWM1_MODE;
                    Tcmp4 = (2 * PWM_PERIOD) - Tcmp4-1;
                }
            }
        }

        break;
  }

  if(Tcmp4)
  {
      if (PWM4Direction == PWM2_MODE)
      {
        //Set Polarity of CC4 High
        TIM1->CCER &= 0xDFFF;
      }
      else
      {
        //Set Polarity of CC4 Low
        TIM1->CCER |= 0x2000;
      }
      __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4,(uint16_t)Tcmp4);
  }
  __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1,(uint16_t)Tcmp1);
  __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2,(uint16_t)Tcmp2);
  __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3,(uint16_t)Tcmp3);


}
